package com.DataProcess;

import android.content.pm.LabeledIntent;
import android.os.Handler;
import android.os.Message;
import android.util.Log;

import com.pudu.mydemo.entity.Contant;
import com.pudu.pdrobot.App;

import org.json.JSONException;
import org.json.JSONObject;

/**
 * Created by tjpu on 2016/8/30.
 */
public class Robot {
    //-----Structure-------
    public static double WHEELS_THREAD = 0.3836;
    public static double WHEEL_PERIMETER = 0.44;
    public static double WHEEL_RADIUS = 0.07;
    public static double[] UWB_TAG_POS = {-0.155, -0.024, 1.146};
    public static double[] THREE_D = {0.5, 0.5, 1.2};  //三维

    //-----Status-----
    public static String mode = Contant.ACTION_HOME;
    public static double[] speed = {0.0,0.0};
    public static double[] targetSpeed = {0.0,0.0};
    public static double tangentialVel = (speed[0]+speed[1])/2.0;  //中心点切向速度
    public static double axialVel= (speed[1]-speed[0])/WHEELS_THREAD;    //轴向速度

    public static double[] angle = {0.0, 0.0, 0.0};  //姿态角,传感器融合之后
    public static double[] angleVel = {0.0, 0.0, 0.0}; //姿态角速度，,传感器融合之后
    public static double[] gyroAngle = {0.0, 0.0, 0.0};  //陀螺仪的姿态角
    public static double[] gyroAngleVel = {0.0, 0.0, 0.0};  //陀螺仪的姿态角速度

    public static double[] position = {0.0, 0.0, 0.0};
    public static double[] dir = {0.0, 0.0, 0.0};
    public static double[] tagPosition = {0.0, 0.0, 0.0};
    public static double[] aimPoint = {0.0, 0.0, 0.0};

    public static int Power = 100;

    //-----  Flag   ----------
    public static boolean isAuto = false;
    public static boolean isEnable = false;
    public static boolean isCloseLoop = true;
    public static boolean isInitPosSucceed = false;
    public static boolean isInitNative = false;
    public static boolean isLowPower = false;
    public static boolean isObstacle = false;
    public static boolean isInsideSource = false;

    //-----
    public static double[] map;
    public static double[] track;
    public static double[] trackWeight;
    public static double[] sourcePos;
    public static double[] uwbStation;

    //------ Control
    public static double[] PWM = {0.0,0.0};
    public static double MAX_CLIENT_SPEED = 0.5;  //最大遥控速度

    //------ Configure
    public static double P_speed = 80;////200.0;
    public static double I_speed = 25;//50.0;
    public static double D_speed = 0.0;
    public static double P_track = 1.5; //1.0;
    public static double D_track = 0.3; //0.0;
    public static double foresee = 0.7;//0.6;
    public static double a_b = 0.3;  //0.5;
    public static double setSpeed = 0.4;//0.45;
    public static double K0 = 1.0;
    public static double K1 = 1.0;
    public static double K2 = 20.0;
    public static double K3 = 20.0;
    public static double K4 = 20.0;
    public static double K5 = 0.030;
}
